#include <EEPROM.h>
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*===========================================================================
 VOORBEREIDING
 ===========================================================================*/
/* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
        pinnen
 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .*/
//Sensor lezen
int multiOut[3] = {2,3,4}; // de digital outs voor multiplexer
int analogPin = A0;
int juisteSensor[8] = { 0, 2, 1, 4, 6, 7, 5, -1 }; // omdat sensoren niet in volgorde zijn aangesloten aan multiplexer

//Algemeen
int ledPin = 13;

//Motor driver
int PWM1 = 5;
int M1 = 8;

int PWM2 = 6; // links
int M2 = 12;




/* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
        sensoren
 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .*/
//Sensor Calibratie
int sensorMinimum[7] = { 1023, 1023, 1023, 1023, 1023, 1023, 1023 };
int sensorMaximum[7] = { 0, 0, 0, 0, 0, 0, 0};

int sensorMinimumPlus = 300; //dusde minimum waarde verhogen zodat waarden dicht bij minimum eerder 0 zullen zijn
int sensorMaximumMin = 50; //waarden dicht bij maximum => maximum

int sensorMinimumMap = 0; // onze gewenste minimum na calibratie
int sensorMaximumMap = 1000; // ons gewenste maximum na calibratie



//Sensor storage
const int sensorCacheSize = 10;
const int sensorCacheArraySize = 7;
int sensorCache[sensorCacheSize*sensorCacheArraySize];
int nSensorCache = 0; // huidig element

uint64_t sensorCacheTimeVar = 0;
unsigned int sensorCacheTimeDelta = 200;
int curSensors[7] = {0, 0, 0, 0, 0, 0, 0};

//rest
int sensorMinimumDigitaal = 700;
//eeprom
void saveInt(int n, int integer)
{
  EEPROM.write(n*2, highByte(integer));
  EEPROM.write(n*2+1, lowByte(integer));
}
int readInt(int n)
{
  return word( EEPROM.read(n*2), EEPROM.read(n*2+1) );
}
void calibratieReadEEPROM()
{
  for(int i = 0; i < 7; i++)
  {
    sensorMinimum[i] = readInt(i);
  }
  for(int i = 7; i < 14; i++)
  {
    sensorMaximum[i] = readInt(i-7);
  }
}
void calibratieSaveEEPROM()
{
  for(int i = 0; i < 7; i++)
  {
    saveInt(i, sensorMinimum[i]);
  }
  for(int i = 7; i < 14; i++)
  {
    saveInt(i, sensorMaximum[i-7]);
  }
}
/* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
        motoren
 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .*/
int motorSnelheid = 255;
int motorMaximumRange = 55;
int motorMinimumRange = 200;



/* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
        pid waarden
 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .*/
const float PIDsettings[3][3] = { 
  {
    4.5, 0.8, 1.5  }
  , {
    2.5,0.95, 4.105  } 
  , {
    1.0, 1.0, 1.0 }
};
const char PIDsetting = 2;
const float kP = PIDsettings[PIDsetting][0];    //proportioneel: is gewoon hoe fout hij zit. Net zoals de 2 sensor robot van school.
const float kI = PIDsettings[PIDsetting][1];     //integraal: wordt groter naargelang de tijd
const float kD = PIDsettings[PIDsetting][2];     //afgeleide: vergelijkt verschil foutmarge
const float PID_I_constant = 0.7; // wordt vermenigvuldigd met P => "geheugenverlies"

float PID_I = 0;
float PID_P_PREV = 0;
int sensorMinimumLijn = 200; // belangrijk voor P te berekenen, schatten waar de lijn is, bepalen of de binnenste sensoren = maximum mogen zijn

//cache circular buffer...
const int pCacheSize = 10;
float pCache[pCacheSize];
int npCache = 0; // huidig element

unsigned int pCacheTimeDelta = 200; // hoeveel ms tussen opslaan van P term
uint64_t pCacheTimeVar = 0; // wordt gebruikt om tijd op te slaan, nooit veranderen!



/*===========================================================================
CACHE
 ===========================================================================*/
void storeSensor(int* values)
{
    for(int i = 0; i < sensorCacheArraySize; i++)
    {
        sensorCache[nSensorCache + i] = values[i];
    }
    nSensorCache = (nSensorCache + sensorCacheArraySize) % (sensorCacheSize * sensorCacheArraySize);
}


int *getSensorCache(int j)
{
    return &sensorCache[((nSensorCache + j) % sensorCacheSize)*sensorCacheArraySize];
}

void storeP(float value)
{
    pCache[npCache++ % pCacheSize] = value;
    npCache %= pCacheSize;
}

float getPCache(int j)
{
    return pCache[(npCache + j) % pCacheSize];
}
void initializeCaches()
{
  for(int i = 0; i < pCacheSize; i++)
  {
    storeP(0);
  }
  int nulSensoren[7] = {0, 0, 0, 0, 0, 0, 0};
  for(int i = 0; i <sensorCacheArraySize; i++)
  {
    storeSensor(nulSensoren);
  }
}
/*===========================================================================
 SETUP
 ===========================================================================*/
void setup()
{
  Serial.begin(9600);
  initializeCaches();
  pinMode(ledPin, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  for(int i = 0; i < 3; i++)
  {
    pinMode(multiOut[i], OUTPUT); // de digitale outputs naar multiplexer
  }
  calibratieReadEEPROM();
  Serial.println("Calibratie waarden aan het lezen uit EEPROM");
  Serial.print("EEPROM: Minimum calibratie:   ");
  for(int i = 0; i < 7; i++)
  {
    int waarde = sensorMinimum[i];
    int spaties = 4 - (int)log10(waarde) + 1;
    String spaces = "";
    for(int s = 0; s < spaties; s++)
      spaces += " ";
      Serial.print(i);
      Serial.print(" => ");
      Serial.print(waarde);
      Serial.print(spaces);
  }
  Serial.println(" ");
  Serial.print("EEPROM: Maximum calibratie:   ");
  for(int i = 0; i < 7; i++)
  {
    int waarde = sensorMaximum[i];
    int spaties = 4 - (int)log10(waarde) + 1;
    String spaces = "";
    for(int s = 0; s < spaties; s++)
      spaces += " ";
      Serial.print(i);
      Serial.print(" => ");
      Serial.print(waarde);
      Serial.print(spaces);
  }
  Serial.println(" ");
  
}
/*===========================================================================
MOTOR FUNCTIES
 ===========================================================================*/
void setMotors(int links, int rechts)
{
  // zorgen dat het [0,255] is

  digitalWrite(M1, (rechts > 0) ? LOW : HIGH); // LOW IS VOORUIT
  digitalWrite(M2, (links > 0) ? LOW : HIGH);
  links = constrain(abs(links), 0, 255);
  rechts = constrain(abs(rechts), 0, 255);
  analogWrite(PWM2, links);
  analogWrite(PWM1, rechts);
}
void setMotorsPercentage(float percent)
{
    percent = constrain(percent, -1.0, 1.0);
    float baseSpeed = 80.0;
    float percentAbs = fabs(percent);
    float maxSpeed = 90.0;
    float minSpeed = 0.0;
    if(percent < 0.0)
    {
        setMotors((int)(baseSpeed - (baseSpeed-minSpeed)*fabs(percent)), (int)(baseSpeed + (maxSpeed-baseSpeed)*fabs(percent)));
    }
    else
    {
        setMotors((int)(baseSpeed + (maxSpeed-baseSpeed)*fabs(percent)), (int)(baseSpeed - (baseSpeed-minSpeed)*fabs(percent)));
    }
}
/*===========================================================================
SENSOREN
 ===========================================================================*/
 /* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
        gewone leesfuncties
 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .*/
int leesSensor(int sensor)
{

  for(int i = 0; i < 3; i ++)
  {
    digitalWrite(multiOut[i], bitRead(juisteSensor[sensor], i));
  }

  return (1023 - analogRead(analogPin));
}

/* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
        sensor calibratie en lezing
 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .*/
void sensorCalibratie(int iDelay, int aantalMillis)
{
  Serial.print("Start calibratie gedurende: ");
  Serial.print(aantalMillis);
  Serial.println("ms");
  digitalWrite(ledPin, HIGH); // led aanzetten

  for(int i = 0; i < 7; i++) // eerst calibratie resetten
  {
    sensorMinimum[i] = 1023;
    sensorMaximum[i] = 0;
  }

  uint64_t beginTijd = millis();

  while( (millis() - beginTijd) < aantalMillis) // gedurende aantalMillis ms minimum en maximum zoeken van iedere sensor
  {
    unsigned long long verlopen = (millis() - beginTijd);
    /*if((verlopen/3) <= aantalMillis)
    {
      Serial.println("links");
      setMotors(-100,100);
    }
    else
    {
      /*Serial.println("rechts");
      setMotors(100,-100);
    }*/
    for(int i = 0; i < 7; i++)
    {
      int sensorWaarde = leesSensor(i);

      if( sensorWaarde < sensorMinimum[i] )
        sensorMinimum[i] = sensorWaarde;

      if( sensorWaarde > sensorMaximum[i] )
        sensorMaximum[i] = sensorWaarde;
    }
    delay(iDelay);
  }
  setMotors(0,0);

  for(int i = 0; i < 7; i++) // zodat hij op de achtergrond eerder 0 zou geven of sneller een witte lijn zou herkennen
  {
    sensorMinimum[i] += sensorMinimumPlus;
    sensorMaximum[i] -= sensorMaximumMin;
  }
  Serial.println("Opslaan EEPROM");
  //calibratieSaveEEPROM();
  digitalWrite(ledPin, LOW); // led afzetten
  Serial.println("Einde calibratie");
    Serial.print("Minimum calibratie:   ");
  for(int i = 0; i < 7; i++)
  {
    int waarde = sensorMinimum[i];
    int spaties = 4 - (int)log10(waarde) + 1;
    Serial.print(i);
    Serial.print(" = ");
    Serial.print(waarde);
    Serial.print("  ");
    for(int s = 0; s < spaties; s++)
      Serial.print(" ");
  }
  Serial.println(" ");
  Serial.print("Maximum calibratie:   ");
  for(int i = 0; i < 7; i++)
  {
    int waarde = sensorMaximum[i];
    int spaties = 4 - (int)log10(waarde) + 1;
    Serial.print(i);
    Serial.print(" = ");
    Serial.print(waarde);
    Serial.print("  ");
    for(int s = 0; s < spaties; s++)
      Serial.print(" ");
  }
  Serial.println(" ");
}


int leesSensorCalibrated(int sensor)
{
  int waarde = leesSensor(sensor);
  waarde = map(waarde, sensorMinimum[sensor], sensorMaximum[sensor], sensorMinimumMap, sensorMaximumMap);
  waarde = constrain(waarde, 0, 1000);
  return waarde;
}
void leesSensorenCalibrated()
{
  for(int i = 0; i < 7; i++)
  {
    curSensors[i] = leesSensorCalibrated(i);
  }
  if( (millis() - sensorCacheTimeVar) >= sensorCacheTimeDelta)
  {
    sensorCacheTimeVar = millis();
    storeSensor(curSensors);
  }
  
}

/*===========================================================================
 PID BEREKENEN
 ===========================================================================*/
float calcP()
{
  int sensorC[7];
  for(int i = 0; i < 7; i++)
    sensorC[i] = curSensors[i];
  
  //waarde van binnenste sensoren moet groter of gelijk zijn aan waarde van buitenste
  if(sensorC[0] >= sensorMinimumLijn)
    sensorC[1] = sensorMaximumMap;
  if(sensorC[1] >= sensorMinimumLijn)
    sensorC[2] = sensorMaximumMap;
    
  if(sensorC[6] >= sensorMinimumLijn)
    sensorC[5] = sensorMaximumMap;
  if(sensorC[5] >= sensorMinimumLijn)
    sensorC[4] = sensorMaximumMap;
  
  //P BEREKENEN
  float P = 0;
  
  for(int i = 0; i < 7; i++)
  {
    if(i != 3)
    {
      int val = ((i <= 2) ? (-1) : (1)) * sensorC[i];
      P += (float)val;
    }
  }
  
  return P;
}
float calcI(float curI, float P, boolean resetTekenWissel)
{
  if(resetTekenWissel)
  {
    if( (curI > 0.0) != (P > 0.0) )
      curI = 0.0;
  }
  float retI = curI*PID_I_constant + P;
  return retI;
}

float calcD(float curP, float prevP)
{
  return (curP - prevP);
}

float calcPID(float KP, float KI, float KD)
{
  float Pterm = calcP();
  float Iterm = calcI(PID_I, Pterm, true);
  PID_I = Iterm;
  float Dterm = calcD(Pterm, PID_P_PREV);
  
  if( (millis() - pCacheTimeVar) >= pCacheTimeDelta)
  {
    pCacheTimeVar = millis();
    storeP(Pterm);
  }
  
  float PID = KP * Pterm + KI * Iterm + KD * Dterm;
  
  PID_P_PREV = Pterm;
  return PID;
  
}
boolean inOnderbreking = false;
boolean isOnderbreking(byte sensorByte)
{
  if(sensorByte != 0B0000000)
  {
    return false;
  }
  float maximumWaardeRechtdoor = 1200;
  int maxError = 3;
  int Error = 0;
  for(int i = (pCacheSize-1); i >= 0 ; i--)
    if( (  (abs(getPCache(i))) > maximumWaardeRechtdoor  ) )
    {
      Error++;
    }
  if(Error >= maxError)
  {
      return false;
  }
  return true;
}
boolean inKruispunt = false;
boolean isKruispunt(byte sensorByte)
{
  int maxError = 3;
  int Error = 0;
  float maximumWaardeRechtdoor = 1200;
  for(int i = (pCacheSize-1); i >= 0 ; i--)
    if( (  (abs(getPCache(i))) > maximumWaardeRechtdoor  ) )
    {
      Error++;

    }
  if(Error >= maxError)
  {
      return false;
  }
    int gevallen = 13;
    byte kruispunt[] = {
    0B1001000,
    0B0001001,
    0B1101000,
    0B0001011,
    0B0101000,
    0B0001010,
    0B1001001,
    0B0101010,
    0B1101001,
    0B1001011,
    0B0101011,
    0B1101010,
    0B1101011
  };
  for(int i = 0; i < gevallen; i++)
  {
      if(kruispunt[i] == sensorByte)
      {
        return true;
      }
  }
  
  return false;
}

byte convertCurSensorToByte()
{
  boolean digitaalSensoren[7];
  
  for(int i = 0; i < 7; i ++)
  {
    boolean boolreading = (curSensors[i] >= sensorMinimumDigitaal);
    digitaalSensoren[i] = boolreading;
  }
  byte sensorByte = digitaalSensoren[0] ? 1 : 0;
  for(int i = 1; i < 7; i ++)
  {
    sensorByte = (sensorByte << 1) | (digitaalSensoren[i] ? 1 : 0);
  }
  return sensorByte;
}
const int ONDERBREKING = 1;
const int KRUISPUNT = 2;
const int NORMAAL = 0;
int checkExceptions() // BIJ KRUISPUNT NOG CHECKEN OF WE AL TIJDJE RECHT RIJDEN
{
  byte curSensorsB = convertCurSensorToByte();
  boolean kruispunt = isKruispunt(curSensorsB);
  if(kruispunt)
  {
    return 1;
  }
  boolean onderbreking = isOnderbreking(curSensorsB);
  if(onderbreking)
  {
    return 2;
  }
  return -1;
}
unsigned long long onderbrekingMillis = 0;
void mainFunctie(boolean checkUitzonderingen)
{
  leesSensorenCalibrated();
  int uitzondering = NORMAAL;
  if(checkUitzonderingen)
  {
    uitzondering = checkExceptions();
  }
  if(uitzondering == NORMAAL)
  {
    float huidigPID = calcPID(kP, kI, kD);
    float variatie = 3000.0;
    float motorKrachtP = map(huidigPID, (-1)*variatie, variatie, -1.0, 1.0);
    setMotorsPercentage(motorKrachtP);
  }
  if (uitzondering == ONDERBREKING)
  {
    /*if(inOnderbreking == false)
    {
     onderbrekingMillis = millis();
     inOnderbreking = true; 
    }
    setMotorsPercentage(sin(0.0065 *((float)(millis() - onderbrekingMillis))));*/
    setMotors(255,255);
    
  }
  else
  {
    inOnderbreking = false;
  }
  if (uitzondering == KRUISPUNT)
  {
    setMotors(255,255);
  }
}

/*===========================================================================
 TEST FUNCTIES
 ===========================================================================*/
void enkelP(float KP)
{
  leesSensorenCalibrated();
  float Pwaarde = calcP();
  Serial.println(Pwaarde);
  /*Serial.print("P = ");
  Serial.println(Pwaarde);*/
  //Serial.println(constrain(Pwaarde/3000.0,-1.0,1.0));
  //Serial.println(constrain((Pwaarde*1.3)/3000.0,-1.0,1.0));
  //Serial.println(constrain((Pwaarde*1.3)/3000.0,-1.0,1.0));
  /*float motorsP = constrain((Pwaarde*1.3)/3000.0,-1.0,1.0);
  if(motorsP < 0.0)
    setMotors(-50,-50);
  else
    setMotors(50,50);*/
  //setMotorsPercentage(constrain((Pwaarde*1.3)/3000.0,-1.0,1.0));
}
void printSensoren(boolean gebruikcalibrated)
{
  for(int i = 0; i < 7; i++)
  {
    int waarde = (gebruikcalibrated) ? leesSensorCalibrated(i) : leesSensor(i);
    int spaties = 4 - (int)log10(waarde) + 1;
      Serial.print(i);
      Serial.print(" = ");
      
      Serial.print(waarde);
      Serial.print("  ");
      for(int s = 0; s < spaties; s++)
        Serial.print(" ");
  }
  Serial.println(" ");
}
/*===========================================================================
 LOOP
 ===========================================================================*/
 float vorigePID = 0.0;
boolean laatstelinks = false;
void loop()
{
  
  if(millis() < 2000)
  {
    sensorCalibratie(10,6000);
    setMotors(20,20);
    delay(500);
    setMotors(0,0);
    delay(1000);
  }
  

  //printSensoren(true);
  /*setMotorsPercentage(-1.0);
  delay(1000);
  setMotorsPercentage(1.0);
  delay(1000);*/
  //setMotorsPercentage(sin((2*PI*millis())/1000));
  //float startmillis = millis();
  /*int sensorLinks = leesSensorCalibrated(2);
  int sensorRechts = leesSensorCalibrated(4);
  if(sensorLinks < 200 && sensorRechts < 200)
  {
    setMotors(70,70);
  }
  else if(sensorLinks > 100)
  {
    laatstelinks = true;
    setMotors(0,70);
  }
  else if (sensorRechts > 100)
  {
    laatstelinks = false;
    setMotors(70,0);
  }
  else if (laatstelinks == true)
  {
    setMotors(0,70);
  }
  else if (laatstelinks = false)
  {
    setMotors(70,0);
  }
  */
  /*
  onderbreking:
  if(midden < 400)
  {
    setMotors(60,60);
  }
  
  */
  int basisRechtdoor = 90;
  int basisDraaiSnel = 100;
  int basisDraaiTraag = 45;
  
  int sensoren[7] = {0, 0, 0, 0, 0, 0, 0};
  for(int i = 0; i < 7; i++)
  {
    sensoren[i] = leesSensorCalibrated(i);
  }
  int P;
  for(int i = 0; i < 7; i++)
  {
    if( i == 0 or i == 1 or i==2)
    {
      P -= sensoren[i];
    }
    else if( i == 4 or i == 5 or i == 6)
    {
      P += sensoren[i];
    }
  }
  int linker = leesSensorCalibrated(2);
  int rechter = leesSensorCalibrated(4);
  int midden = leesSensorCalibrated(3);
  if(midden > 500)
  {
    if((-P) >= 100) // naar links
    {
      laatstelinks = true;
      setMotors(basisDraaiTraag,basisDraaiSnel);
    }
    else if ((P) >= 100) // naar rechts
    {
      laatstelinks = false;
      setMotors(basisDraaiSnel,basisDraaiTraag);
    }
    else
    {
      setMotors(basisRechtdoor,basisRechtdoor);
    }
  }
  else
  {
    if(laatstelinks)
    {
      setMotors(basisDraaiTraag,basisDraaiSnel);
    }
    else
    {
      setMotors(basisDraaiSnel,basisDraaiTraag);
    }
  }
  
  if(false)
  {
    leesSensorenCalibrated();
    float PID = calcPID(1.0, 0.0, 0.0);
    
    //Serial.println(PID);
    /*if(leesSensorCalibrated(3) >= 500)
    {
      PID = constrain(vorigePID*1000, -3000, 3000);
    }*/
    float aPID = fabs(PID);
    
    float factorP = 0.5;
    if(aPID < 500)
    {
      factorP = 0.5;
    }
    else if (aPID < 1000)
    {
      factorP = 1.0;
    }
    else if (aPID < 1500)
    {
      factorP = 1.5;
    }
    else if (aPID < 2000)
    {
      factorP = 2.0;
    }
    else if (aPID < 2500)
    {
      factorP = 2.0;
    }
    else
    {
      factorP = 2.0;
    }
    Serial.print("echte huidige PID = ");
    Serial.print(PID);
    vorigePID = PID;
    Serial.print("   vorige PID = ");
    Serial.print(vorigePID);
    Serial.print("   huidige PID = ");
    Serial.println(PID);
  
    
    
    setMotorsPercentage((factorP*PID)/3000.0);
    int basis = 200;
  }
}

